clc; close all; clear;

% Constant
DIM = 2;
X_COMP = 1;
Y_COMP = 2;

% Cylinder
D = 1.0;
C = zeros(DIM, 1);

% Domain
Lx = 16 * D;
Ly = 8 * D;

x0 = -4 * D;
y0 = -4 * D;

x1 = x0 + Lx;
y1 = y0 + Ly;

% Grid
Nx = 64;
Ny = 32;

hx = Lx / Nx; hx2 = hx*hx;
hy = Ly / Ny; hy2 = hy*hy;

dV = hx * hy;

nC = cTotalNum(Nx, Ny);
nF = fTotalNum(Nx, Ny);

% FV connectivity
Sf = zeros(nF, DIM);
snsign = zeros(nC, 4); % E, S, W, N
c2f = zeros(nC, 4);
f2c = zeros(nF, 2);

for j = 1:Ny
    for i = 1:Nx
        cI= cIDX(i, j, Nx, Ny, true);
        fI_E = fIDX(i, j, Nx, Ny, DIR.E);
        fI_W = fIDX(i, j, Nx, Ny, DIR.W);
        fI_N = fIDX(i, j, Nx, Ny, DIR.N);
        fI_S = fIDX(i, j, Nx, Ny, DIR.S);

        Sf(fI_E,:) = [hy, 0];
        Sf(fI_W,:) = [hy, 0];
        Sf(fI_N,:) = [0, hx];
        Sf(fI_S,:) = [0, hx];

        snsign(cI, 1) = 1.0;
        snsign(cI, 2) = -1.0;
        snsign(cI, 3) = -1.0;
        snsign(cI, 4) = 1.0;

        c2f(cI, 1) = fI_E;
        c2f(cI, 2) = fI_S;
        c2f(cI, 3) = fI_W;
        c2f(cI, 4) = fI_N;
    end
end

% Variable
rhoU = zeros(nC, DIM);
rhoU_f = zeros(nF, DIM);
rhoU_star = zeros(nC, DIM);
U_f = zeros(nF, DIM);
tau_f = zeros(nF, DIM, DIM);
p = zeros(nC, 1);
p_f = zeros(nF, 1);

% B.C.
u0 = 1.0;
p0 = 0.0;

% Property
rho = 1.0;
Re = 100;
mu = rho * u0 * D / Re;

% Control
tMax = 10.0;
nMax = 10000;
nOut = 100;
dt = 1e-6;

% Time-marching
t = 0.0;
n = 0;
while t < tMax && n < nMax
    % Predictor
    for j = 1:Ny
        for i = 1:Nx
            cI = cIDX(i, j, Nx, Ny, true);
            
            CONV = zeros(1, DIM);
            DIVTAU = zeros(1, DIM);
            for f = 1:4
                fI = c2f(cI, f);
                Sn = Sf(fI, :) * snsign(cI, f);
                CONV = CONV + dot(rhoU_f(fI, :), Sn) * U_f(fI, :);
                DIVTAU = DIVTAU + transpose(squeeze(tau_f(fI,:,:)) * transpose(Sn));
            end
            
            rhoU_star(cI, :) = rhoU(cI, :) + dt / dV * (DIVTAU - CONV);
        end
    end

    % Corrector
    A = zeros(nC, nC);
    b = zeros(nC, 1);
    for j = 1:Ny
        for i = 1:Nx
            cI = cIDX(i, j, Nx, Ny, true);
            if isBdryCell(i, j, Nx, Ny)
                for f = 1:4
                    D = DIR(f);
                    fI = c2f(cI, f);
                    if isInletFace(i, j, Nx, Ny, D) || isWallFace(i, j, Nx, Ny, D)
                        % zero-gradient
                    elseif isOutletFace(i, j, Nx, Ny, D)
                        % fixed-value
                    else
                        % interior
                    end
                end
            else
                A(i, j) = 2 * dt / hx2 + 2 * dt / hy2;
                A(i, j+1) = -dt / hy2;
                A(i, j-1) = -dt / hy2;
                A(i+1, j) = -dt / hx2;
                A(i-1, j) = -dt / hx2;
                
                cI_E = cIDX(i+1, j, Nx, Ny, true);
                cI_S = cIDX(i, j-1, Nx, Ny, true);
                cI_W = cIDX(i-1, j, Nx, Ny, true);
                cI_N = cIDX(i, j+1, Nx, Ny, true);

                b(i) = -((rhoU_star(cI_E, X_COMP) - rhoU_star(cI_W, X_COMP)) / (2 * hx) + (rhoU_star(cI_N, Y_COMP) - rhoU_star(cI_S, Y_COMP)) / (2 * hy));
            end
        end
    end
    p = A\b;

    % Update counter
    t = t + dt;
    n = n + 1;
end

function n = cTotalNum(Nx,Ny)
    n = Nx * Ny;
end

function n = fTotalNum(Nx,Ny)
    n = cTotalNum(Nx+1, Ny) + cTotalNum(Nx, Ny+1);
end

function n = cIDX(ix,iy,Nx,Ny,cOrder)
    if cOrder
        n = Nx * (iy - 1) + ix;
    else
        n = Ny * (ix - 1) + iy;
    end
end

function n = fIDX(ix, iy, Nx, Ny, D)
    switch(D)
        case DIR.N
            n = cIDX(ix, iy+1, Nx, Ny+1, true); % n = iy * Nx + ix;
        case DIR.S
            n = cIDX(ix, iy, Nx, Ny+1, true); % n = (iy-1) * Nx + ix;
        case DIR.W
            n = cTotalNum(Nx, Ny+1) + cIDX(ix, iy, Nx+1, Ny, false); % n = Nx * (Ny+1) + Ny * (ix-1) + iy;
        case DIR.E
            n = cTotalNum(Nx, Ny+1) + cIDX(ix+1, iy, Nx+1, Ny, false); % n = Nx * (Ny+1) + Ny * ix + iy;
    end
end

function flag = isBdryCell(ix, iy, Nx, Ny)
    flag = (ix == 1) || (ix == Nx) || (iy == 1) || (iy == Ny);
end

function flag = isInletFace(ix, iy, Nx, Ny, D)
    flag = (ix == 1) && (D == DIR.W);
end

function flag = isOutletFace(ix, iy, Nx, Ny, D)
    flag = (ix == Nx) && (D == DIR.E);
end

function flag = isWallFace(ix, iy, Nx, Ny, D)
    isTop = (iy == Ny) && (D == DIR.N);
    isBottom = (iy == 1) && (D == DIR.S);
    flag = isTop || isBottom;
end